# -*- coding: utf-8 -*-
#!/usr/bin/env python

import rospy
from rosgraph_msgs.msg import Clock


uav_id = $id
uav_nums = $num


# 回调函数，处理收到的其他主机时间
def time_callback(msg, id):
    clock_msg = Clock()
    clock_msg.clock = rospy.Time.now()
    other_time = msg.clock
    time_diff = clock_msg.clock - other_time
    print("With Machine:{}, Time difference: {:.6f} seconds".format(id, time_diff.to_sec()))
    
# 主函数
if __name__ == '__main__':
    rospy.init_node('time_diff_checker', anonymous=True)
    
    # 订阅其他 UAV 的时间话题
    for id in range(uav_nums):
        if id == uav_id:
            continue
        # 使用 lambda 函数将 id 传递给回调函数
        rospy.Subscriber('/time_{}'.format(id), Clock, lambda msg, id=id: time_callback(msg, id))

    # 设置发布频率为 30 Hz
    rate = rospy.Rate(30)
    while not rospy.is_shutdown():
        rospy.spin()
